/* 
	IRNode.cpp
	IR reading for roboard serial ports
*/
#include "ros/ros.h"
#include "navigation/IR.h"
#include <string.h>
#include <iostream>
#include <stdlib.h>
#include <csignal>
#include <vector>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>

using namespace navigation;

ros::Publisher ir_pub;
std::vector<std::string> device;
#define NDEV 1

bool run = true;
void sigtrap(int) { run = false; }

int main(int argc, char **argv)
{
	int fd[NDEV];
	device.push_back(std::string("/dev/ttys2"));
	for(int i = 0; i < NDEV; i++) {
		fd[i] = open(device[i].c_str(), O_RDONLY);
		if(fd[i] < 0) {
			ROS_ERROR("serial open");
			exit(-1);
		}
	}

  ros::init(argc, argv, "ir"); 
  ros::NodeHandle n;

  ir_pub = n.advertise<IR>("/serial/ir", 1000);

  signal(SIGINT, sigtrap);
  std::cout << "\nPress ctrl-c to stop the program\n\n";

	ros::Rate loop_rate(1000);
  int ir_interval = 1000;
	struct timeval start, end;

	int BUFSIZE = 32;
	char buf[BUFSIZE];

  while (ros::ok() && run) {
		gettimeofday(&start, NULL);

    std::vector< std::pair<int,int> > ir_query;
    ir_query.resize(5);
    for (unsigned int i = 0; i < ir_query.size(); i++) ir_query[i].first = i;

    //s->readADC(ir_query);
		read(fd[0], buf, BUFSIZE);
		printf("%s\n", buf);

/*
    IR msg;
		msg.timestamp = start.tv_sec+double(start.tv_usec)/1000000.0;
    msg.ir0 = ir_query[0].second;
    msg.ir1 = ir_query[1].second;
    msg.ir2 = ir_query[2].second;
    msg.ir3 = ir_query[3].second;
    msg.ir4 = ir_query[4].second;
		ir_pub.publish(msg);
*/


		while((end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec)) < ir_interval*1000){
			loop_rate.sleep();
			gettimeofday(&end, NULL);
		}

  }

  //s->disconnect();
  return 0;
}
